"""
动作执行器模块

该模块负责执行机器人的动作，包括：
- 动作组的加载和执行
- 单个动作的执行
- 动作队列的管理
"""

import math
import time
import logging
import threading
from typing import Dict, List, Optional, Union
from robot_core.config.servo_config import ServoConfig
from .action_manager import ActionManager
from .action_parser import ActionParser
from .kinematics import Kinematics


class ActionExecutor:
    """动作执行器类"""

    # 机器人腿部参数（单位：mm）
    shoulder_length = 44.60  # 肩关节长度
    thigh_length = 75.00  # 大腿长度
    calf_length = 126.50  # 小腿长度

    def __init__(
        self,
        config: ServoConfig,
        action_dir: str = "actions",
        log_level: int = logging.INFO,
    ):
        """
        初始化动作执行器

        Args:
            config: 舵机配置实例
            action_dir: 动作组文件目录
            log_level: 日志级别
        """
        # 配置日志
        logging.basicConfig(level=log_level)
        self.logger = logging.getLogger(__name__)

        self.config = config
        self.protocol = config.servo_protocol
        self.action_manager = ActionManager(config, action_dir, log_level)
        self.action_parser = ActionParser(config, log_level)
        self.kinematics = Kinematics()

        # 动作执行状态
        self.is_running = False
        self.current_action = None
        self.action_thread = None

    def execute_action(self, action_name: str, retry_count: int = 3) -> bool:
        """
        执行动作

        Args:
            action_name: 动作名称
            retry_count: 重试次数

        Returns:
            bool: 是否执行成功
        """
        if not self.protocol.connect():
            self.logger.error("无法连接到串口设备")
            return False

        try:
            action = self.action_manager.get_action(action_name)
            if not action:
                self.logger.error(f"动作组不存在: {action_name}")
                return False

            self.is_running = True
            for step in action["steps"]:
                if not self.is_running:
                    break

                # 执行每个舵机的动作
                for servo_id, pos, time_ms in step:
                    if not self.protocol.set_servo_angle(servo_id, pos, time_ms):
                        self.logger.error(
                            f"设置舵机角度失败: ID={servo_id}, pos={pos}, time={time_ms}"
                        )
                        return False

                # 等待动作完成
                time.sleep(time_ms / 1000.0)

            return True
        except Exception as e:
            import traceback

            traceback.print_exc()
            self.logger.error(f"执行动作失败: {e}")
            return False
        finally:
            self.is_running = False
            self.protocol.disconnect()

    def _execute_action_thread(self, action: dict, repeat: int) -> None:
        """
        动作执行线程

        Args:
            action: 动作组数据
            repeat: 重复次数
        """
        try:
            for _ in range(repeat):
                for step in action.get("steps", []):
                    if not self.is_running:
                        break

                    step_type = step.get("type")
                    if step_type == "servo":
                        self._execute_servo_step(step)
                    elif step_type == "leg_position":
                        self._execute_leg_step(step)
                    elif step_type == "camera":
                        self._execute_camera_step(step)
                    elif step_type == "delay":
                        time.sleep(step["duration"] / 1000.0)

        except Exception as e:
            self.logger.error(f"执行动作时出错: {e}")
        finally:
            self.is_running = False
            self.current_action = None

    def _execute_servo_step(self, step: dict) -> bool:
        """
        执行舵机动作

        Args:
            step: 动作步骤

        Returns:
            bool: 是否执行成功
        """
        try:
            servo_id = step["servo_id"]
            position = step["position"]
            speed = step.get("speed", 1000)
            return self.protocol.set_servo_angle(servo_id, position, speed)
        except Exception as e:
            self.logger.error(f"执行舵机动作失败: {e}")
            return False

    def _execute_leg_step(self, step: dict) -> bool:
        """
        执行腿部动作

        Args:
            step: 动作步骤

        Returns:
            bool: 是否执行成功
        """
        try:
            leg = step["leg"]
            position = step["position"]
            speed = step.get("speed", 1000)
            return self.action_parser.parse_command(
                "move_leg", [leg, position[0], position[1], position[2], speed]
            )
        except Exception as e:
            self.logger.error(f"执行腿部动作失败: {e}")
            return False

    def _execute_camera_step(self, step: dict) -> bool:
        """
        执行云台动作

        Args:
            step: 动作步骤

        Returns:
            bool: 是否执行成功
        """
        try:
            servo_id = step["servo_id"]
            position = step["position"]
            speed = step.get("speed", 500)
            min_angle = step.get("min_angle", 500)
            max_angle = step.get("max_angle", 2500)

            # 限制位置范围
            position = max(min_angle, min(max_angle, position))
            return self.protocol.set_servo_angle(servo_id, position, speed)
        except Exception as e:
            self.logger.error(f"执行云台动作失败: {e}")
            return False

    def stop_action(self) -> None:
        """停止当前动作"""
        if self.is_running:
            self.is_running = False
            self.logger.info("动作已停止")

    def get_status(self) -> Dict[str, Union[bool, str, None]]:
        """
        获取执行器状态

        Returns:
            Dict[str, Union[bool, str, None]]: 状态信息
        """
        return {"is_running": self.is_running, "current_action": self.current_action}

    def execute_command(self, cmd: str, params: List[Union[int, float, str]]) -> bool:
        """
        执行命令

        Args:
            cmd: 命令类型
            params: 命令参数

        Returns:
            bool: 是否执行成功
        """
        return self.action_parser.parse_command(cmd, params)

    def set_leg_position(self, leg: int, position: List[float], speed: int) -> None:
        """
        计算并设置单条腿的舵机角度

        Args:
            leg: 腿的编号(0-5)
            position: [x, y, z]坐标，表示足端位置
            speed: 运动速度(ms)
        """
        print(f"计算腿 {leg} 的舵机角度，目标位置: {position}")

        # 角度转换因子
        factor = 180 / math.pi / 0.24

        # 计算第一个关节角度
        angle1 = math.atan(position[1] / position[0])

        # 计算腿长
        leg_length = position[1] / math.sin(angle1)

        # 计算中间变量
        temp = math.pow(position[2], 2) + pow(leg_length - self.shoulder_length, 2)
        ft = math.sqrt(temp)

        # 计算第二个关节角度
        leg_angle1 = math.atan(position[2] / (leg_length - self.shoulder_length))
        leg_angle2 = math.acos(
            (
                math.pow(self.thigh_length, 2)
                + math.pow(ft, 2)
                - math.pow(self.calf_length, 2)
            )
            / (2 * self.thigh_length * ft)
        )
        leg_angle2 = leg_angle1 + leg_angle2

        # 计算第三个关节角度
        leg_angle3 = math.acos(
            (
                math.pow(ft, 2)
                - math.pow(self.thigh_length, 2)
                - math.pow(self.calf_length, 2)
            )
            / (2 * self.thigh_length * self.calf_length)
        )

        # 转换为舵机角度值
        if leg < 3:
            servo1_pos = int(313 + leg_angle1 * factor)
            servo2_pos = int(500 - leg_angle2 * factor)
            servo3_pos = int(687 - leg_angle3 * factor - 5)
        else:
            servo1_pos = int(687 - leg_angle1 * factor)
            servo2_pos = int(500 + leg_angle2 * factor)
            servo3_pos = int(313 + leg_angle3 * factor + 5)

        # 设置舵机位置
        servo_base = leg * 3 + 1
        print(f"设置舵机位置 - ID: {servo_base}, {servo_base+1}, {servo_base+2}")
        print(f"目标位置: {servo1_pos}, {servo2_pos}, {servo3_pos}")

        self.protocol.set_servo_angle(servo_base, servo1_pos, speed)
        self.protocol.set_servo_angle(servo_base + 1, servo2_pos, speed)
        self.protocol.set_servo_angle(servo_base + 2, servo3_pos, speed)

    def stand_up(self, speed: int = 1000) -> None:
        """
        初始化六足机器人站立姿势

        Args:
            speed: 运动速度(ms)，默认1000ms
        """
        print("初始化六足机器人站立姿势...")
        # 初始化六足机器人站立姿势
        for leg in range(6):
            position = [100.0, 100.0, -70.0]
            self.set_leg_position(leg, position, speed)
        time.sleep(speed / 1000.0)

    def stop_action(self) -> None:
        """停止当前动作"""
        if self.is_running:
            self.is_running = False
            self.config.stop()
